#include "SG805RobotRos.h"

// 机械臂实例
SG805Robot *sg805RobotPtr = new SG805Robot();

// SG805Robot 监听友元函数
void listening(SG805Robot *p)
{
    // std::cout << "void listening \n";
    p->listening();
}

SG805RobotRos::SG805RobotRos()
{
    // 句柄实例
    ros::NodeHandle nh;

    // 动作名称
    action_name = "/sg805/sg805_controller/follow_joint_trajectory";

    // 初始化关节变量
    joint_msg.name.resize(6);
    joint_msg.position.resize(6);
    joint_msg.header.frame_id = "/sg805";

    // 初始化ros_feedback
    ros_feedback.header.frame_id = "/sg805";
    ros_feedback.desired.positions.resize(6);
    ros_feedback.actual.positions.resize(6);

    // 关节命名
    joint_msg.name[0] = "joint1";
    joint_msg.name[1] = "joint2";
    joint_msg.name[2] = "joint3";
    joint_msg.name[3] = "joint4";
    joint_msg.name[4] = "joint5";
    joint_msg.name[5] = "joint6";

    // 各个关节执行完毕所需的时间
    memset(durations, 0, sizeof(durations));
    duration_sum = 0;

    // 功能
    extra_features_msg.Tag = 0;
    extra_features_msg.Position.resize(6);

    // 单点功能
    fast_point_msg.Mode = 0;
    fast_point_msg.duration = 0;
    fast_point_msg.Position.resize(6);

    // 启动
    sg805RobotPtr->startConstruction();

    // 监听机械臂
    std::thread t_listening = std::thread(listening, sg805RobotPtr);

    // 关节发布者初始化
    joint_pub = nh.advertise<sensor_msgs::JointState>("/sg805/joint_states", 1);

    // 服务器初始化
    as = new Server(nh, action_name, boost::bind(&SG805RobotRos::executeCB, this, _1), false);

    // 服务器开启
    as->start();

    // 功能订阅者初始化
    extra_features_sub = nh.subscribe("/sg805/extraFeatures", 1, &SG805RobotRos::extraFeaturesCB, this);
    fast_point_sub = nh.subscribe("/sg805/fastPoint", 1, &SG805RobotRos::fastPointCB, this);

    // srv服务器挂起
    srv_server = nh.advertiseService("/sg805/distance", &SG805RobotRos::getDistanceCB, this );

    ros::AsyncSpinner spinner(1);
    spinner.start();


    // 获取机器人各个关节的脉冲角度
    // sg805RobotPtr->getEncoders();

    sg805RobotPtr->getPose();
    usleep(100000); // 0.1s间隔
    // 写入到控制器中
    sg805RobotPtr->location_setting();

    usleep(100000); // 0.1s间隔


    // 更新关节数据
    jointStateUpdate();

    if (t_listening.joinable())
    {
        t_listening.join();
    }
    else
    {
        std::cout << "thread cannot join" << std::endl;
    }
    std::cout << "退出\n";
    ros::shutdown();
}

SG805RobotRos::~SG805RobotRos()
{
}

// extraFeatures功能回调函数
void SG805RobotRos::extraFeaturesCB(const sg805_driver::ExtraFeaturesConstPtr &msg)
{
    // 获取消息标签
    extra_features_msg.Tag = msg->Tag;

    switch (msg->Tag)
    {
    case STOPPED:
        // 紧急制动
        sg805RobotPtr->robot_stop();
        break;

    case UPLOAD_START:
        // 重新开启Location数据的上传，默认开启
        sg805RobotPtr->upload_start();
        break;

    case UPLOAD_STOP:
        // 关闭Location数据的上传，默认开启
        sg805RobotPtr->upload_stop();
        break;

    case PWM_START:
        // 获取数据
        sg805RobotPtr->pwm_handle.PSC = msg->PSC;
        sg805RobotPtr->pwm_handle.ARR = msg->ARR;
        sg805RobotPtr->pwm_handle.CCR1 = msg->CCR1;
        sg805RobotPtr->pwm_handle.PluseCount = msg->PluseCount;
        // 发送pwm数据
        sg805RobotPtr->pwm_start();
        break;

    case PWM_STOP:
        // 关闭pwm
        sg805RobotPtr->pwm_stop();
        break;

    case PIN0_ON:
        sg805RobotPtr->pin0_on();
        break;

    case PIN0_OFF:
        sg805RobotPtr->pin0_off();
        break;

    case PIN1_ON:
        sg805RobotPtr->pin1_on();
        break;

    case PIN1_OFF:
        sg805RobotPtr->pin1_off();
        break;

    case TOGGLE_ENABLE_PINS: // 12
        /*
         * 关节序号  0   1   2   3   4   5   6   7     8pwm    全部
         * 输入数值  1   2   4   8   16  32  64  128   256     (511)0x1ff
         *
         * 可以进行各种使能组合,不过为了方便,建议在终端操作时,采用上述几个数值
         */

        sg805RobotPtr->enable_pins = msg->PSC & 0x01ff;
        cout << "sg805RobotPtr->enable_pins " << sg805RobotPtr->enable_pins << endl;
        sg805RobotPtr->toggle_enable_pins();
        break;

    case USART_START: // 13
        // 开启串口通信
        sg805RobotPtr->usart_start();
        break;

    case USART_STOP: // 14
        // 关闭USART通信中断
        sg805RobotPtr->usart_stop();
        break;

    case RS485_ENABLE: // 17
        sg805RobotPtr->rs485_enable();
        break;

    case RS485_DISABLE: // 18
        sg805RobotPtr->rs485_disable();
        break;

    case LOCATION_SETTING:
        // 设置当前角度脉冲数值,必须要在机械臂完全停止运动的时候使用这个功能
        sg805RobotPtr->location_setting_handle.state = LOCATION_SETTING;

        // 6轴
        sg805RobotPtr->location_setting_handle.position[0] = msg->Position[0];
        sg805RobotPtr->location_setting_handle.position[1] = msg->Position[1];
        sg805RobotPtr->location_setting_handle.position[2] = msg->Position[2];
        sg805RobotPtr->location_setting_handle.position[3] = msg->Position[3];
        sg805RobotPtr->location_setting_handle.position[4] = msg->Position[4];
        sg805RobotPtr->location_setting_handle.position[5] = msg->Position[5];

        // 调用API
        sg805RobotPtr->location_setting();
        break;

    case GET_ENCODER: // 21   //获取编码器数据
        sg805RobotPtr->getEncoders();
        break;

    case GET_POSE: // 22   //获取编码器数据，并转化为机器人的当前位置
        sg805RobotPtr->getPose();
        usleep(1000); // 1ms
        sg805RobotPtr->location_setting();
        break;

    case RETURN_ZERO: // 23
        // 调用函数
        sg805RobotPtr->return_to_zero();
        break;

    case DISABLE_SERVO: // 24
        // 伺服失能
        sg805RobotPtr->disableServo(msg->PSC, msg->ARR);
        break;

    case ENABLE_SERVO: // 25
        // 伺服使能
        sg805RobotPtr->enableServo(msg->PSC, msg->ARR);
        break;

    case SET_SERVO_ID: // 26
        sg805RobotPtr->setServoID(msg->PSC);
        sleep(1);
        break;

    case RESET_TURNS: // 27
        // 清空圈数
        sg805RobotPtr->resetTurns(msg->PSC);
        sleep(1);
        break;

    case GET_SERVO_VERSION: // 28
        // 获取伺服版本
        sg805RobotPtr->getServoVersion(msg->PSC);
        sleep(1);
        break;

    case SET_SERVO_SUBDIVISION: // 29
        // 设置伺服的细分
        sg805RobotPtr->setServoSubdivision(msg->PSC);
        sleep(1);
        break;


        
    default:
        break;
    }
}


//单点功能回调函数
void SG805RobotRos::fastPointCB(const sg805_driver::FastPointConstPtr &msg)
{
    //cout << "fastPointCB :" << endl;
    fast_point_msg.Mode = msg->Mode;

    long temp1,temp2,temp3;

    if (fast_point_msg.Mode == 0)
    {//绝对移动
        for (int i = 0; i < 6; i ++)
        {
            
            sg805RobotPtr->fast_point_handle.position[i] = msg->Position[i] - sg805RobotPtr->location.position[i];

            if (sg805RobotPtr->fast_point_handle.position[i] == 0)
            {
                sg805RobotPtr->fast_point_handle.period[i] = 0;
            }
            else
            {
                temp1 = msg->duration * 100000;
                temp2 = abs(sg805RobotPtr->fast_point_handle.position[i]);
                temp3 = temp1/temp2;
                sg805RobotPtr->fast_point_handle.period[i] = (int)__builtin_sqrt(temp3);
            }
        }
    }
    else if (fast_point_msg.Mode == 1)
    {//增量移动
        /* code */
        for (int i = 0; i < 6; i ++)
        {
            
            sg805RobotPtr->fast_point_handle.position[i] = msg->Position[i];
            if (sg805RobotPtr->fast_point_handle.position[i] == 0)
            {
                sg805RobotPtr->fast_point_handle.period[i] = 0;
            }
            else
            {
                temp1 = msg->duration * 100000;
                temp2 = abs(sg805RobotPtr->fast_point_handle.position[i]);
                temp3 = temp1/temp2;
                sg805RobotPtr->fast_point_handle.period[i] = (int)__builtin_sqrt(temp3);
            }
        }
    }

    for (int i = 0; i < 8; i++)
    {
        cout <<  sg805RobotPtr->fast_point_handle.position[i] << " ";
    }
    cout << endl;
    for (int i = 0; i < 8; i++)
    {
        cout <<  sg805RobotPtr->fast_point_handle.period[i] << " ";
    }
    cout << endl;
    
    //调用单点运动接口
    sg805RobotPtr->fastPointMove();
}

// goal回调函数
void SG805RobotRos::executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
{
    std::cout << "\033[1m\033[32m SG805RobotRos::executeCB start \033[0m 当前机械臂状态为"
              << (int)sg805RobotPtr->location.state << std::endl;

    if (sg805RobotPtr->location.state != STOPPED)
    { // 如果上一个任务没有执行完毕

        std::cout << "上一个任务没有完成" << std::endl;
        ros_result.error_code = ros_result.OLD_HEADER_TIMESTAMP;
        ros_result.error_string = "Action server is busy now, ignore this requestion\n";
        as->setAborted(ros_result);
        // as->setSucceeded(ros_result);
        return;
    }

    duration_total = 0;

    // 有时可能会需要路径点重排列，当如果是固定赋值，则不需要
    if (ros::ok())
    {
        sg805RobotPtr->NumberOfPoints = goal->trajectory.points.size(); // 获取路径点数量

        // 将路点的终点写入ros_feedback中
        ros_feedback.desired.positions[0] = goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 1].positions[0];
        ros_feedback.desired.positions[1] = goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 1].positions[1];
        ros_feedback.desired.positions[2] = goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 1].positions[2];
        ros_feedback.desired.positions[3] = goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 1].positions[3];
        ros_feedback.desired.positions[4] = goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 1].positions[4];
        ros_feedback.desired.positions[5] = goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 1].positions[5];

        /*******************************************************
         *        将路径点赋值 角度获取 单点执行时间获取开始
         *        1 计算出各个路径点的位置信息
         *        2 计算出到达各个路径点需要的运行角度信息          *
         *        3 计算出各个点所花费的时间
         *        4 计算出各个路径点的各个轴运动速度
         *******************************************************/
        // 路径点赋值
        for (int index = 0; index < sg805RobotPtr->NumberOfPoints; index++)
        {
            // 1 计算出各个路径点的位置信息
            for (int i = 0; i < 6; i++)
            {
                // 获取位置信息，也是脉冲信息， 位置 = (物理角度 / PI) * 单位脉冲 + 零点偏移
                sg805RobotPtr->trajectory[index].position[i] =
                    (goal->trajectory.points[index].positions[i] * sg805RobotPtr->plu2angel[i]) / PI + sg805RobotPtr->zeroPlu[i];

            }
            // 2023-6-3
            sg805RobotPtr->trajectory[index].position[5] += 
                (sg805RobotPtr->trajectory[index].position[4]) / 50;

            

            // 2 计算出到达各个路径点需要的运行角度信息
            if (index != 0)
            {

                for (int i = 0; i < 6; i++)
                {
                    delta_postions[i] =
                        sg805RobotPtr->trajectory[index].position[i] -
                        sg805RobotPtr->trajectory[index - 1].position[i];
                }
            }

            /*******************************************************
             *         3 计算出各个点所花费的时间                      *
             *******************************************************/
            // 第一个轨迹点，各个关节速度为0
            if (index == 0)
            {
                for (int i = 0; i < 6; i++)
                {
                    durations[i] = 0;
                }

                /***************************************************
                 * 第一个点是机械臂当前位置点,因此机械臂到达此点花费的时间0  *
                 ****************************************************/
                // 获得此点运行的平均时间
                duration_mean = 0;
                // 时间记录
                duration_sum = 0;
                // 有效时间数据个数
                numberOfValidDuration = 0;
            }

            // 最后一个点
            else if (index == sg805RobotPtr->NumberOfPoints - 1)
            {
                // 最后一个点 各个轴的情况
                for (int i = 0; i < 6; i++)
                {
                    // 最后一个轨迹点速度为0，所以要执行这个点，则必须和前一个点的速度保持一致
                    if (goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 2].velocities[i] == 0)
                    { // 如果倒数第二个点已经速度为0的话，那么最后一个点和前面保持一致
                        durations[i] = 0;
                    }
                    else
                    {
                        // 如果此关节的位置未发生变化
                        if (delta_postions[i] == 0)
                        {
                            durations[i] = 0;
                        }

                        // 如果位置发生变化
                        else
                        {
                            durations[i] =
                                (goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 1].positions[i] -
                                 goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 2].positions[i]) *
                                200000000 / goal->trajectory.points[sg805RobotPtr->NumberOfPoints - 2].velocities[i];

                            // 和前一个点的速度保持一致
                            sg805RobotPtr->trajectory[index].numberOfFullPeriod[i] = sg805RobotPtr->trajectory[index - 1].numberOfFullPeriod[i];
                            sg805RobotPtr->trajectory[index].restPeriod[i] = sg805RobotPtr->trajectory[index - 1].restPeriod[i];
                            sg805RobotPtr->trajectory[index].numberOfPeriod[i] = sg805RobotPtr->trajectory[index - 1].numberOfPeriod[i];
                            sg805RobotPtr->trajectory[index].period[i] = sg805RobotPtr->trajectory[index - 1].period[i];

                            // 累加出6个轴的总时间
                            duration_sum += abs(durations[i]);
                            // 记录有效关节数，防止一些关节不进行运动，而导致执行所需要的平均时间偏低
                            numberOfValidDuration++;
                        }
                    }
                }

                // 获得此点运行的平均时间
                duration_mean = numberOfValidDuration == 0 ? 0 : round(duration_sum / numberOfValidDuration);
                // 时间记录
                duration_sum = 0;
                // 有效时间数据个数
                numberOfValidDuration = 0;
            }

            // 路径中间的点
            else
            {
                // 获得各个轴的
                for (int i = 0; i < 6; i++)
                {
                    // 其余的点，运行时间 = 角度差/速度，  周期 = 运行时间 / 位置差
                    if (goal->trajectory.points[index].velocities[i] == 0)
                    {
                        durations[i] = 0;
                    }
                    else
                    {
                        // 如果位置没有发生变化
                        if (delta_postions[i] == 0)
                        {
                            durations[i] = 0;
                        }
                        else
                        {
                            // 如果位置有变化 算出当前轴执行完当前点，所需要的时间(单位为us)
                            durations[i] =
                                (goal->trajectory.points[index].positions[i] - goal->trajectory.points[index - 1].positions[i]) * 200000000 / goal->trajectory.points[index].velocities[i];

                            // sg805RobotPtr->trajectory[index].period[i] =
                            //     abs((1000000 * PI) / goal->trajectory.points[index].velocities[i] / sg805RobotPtr->plu2angel[i]);

                            // 累加出总时间
                            duration_sum += abs(durations[i]);
                            // 记录有效关节数，防止一些关节不进行运动，而导致执行所需要的平均时间偏低
                            numberOfValidDuration++;
                        }
                    }

                    // 分段获取速度信息
                    // std::cout << "positions[" << i <<"] " << this->goal.points[index].positions[i] <<
                    //      "  Goal : " << goal->trajectory.points[index].positions[i] << endl;
                    // cout << goal->trajectory.points[index].velocities[i] << " ";
                }

                // 获得此点运行的平均时间
                duration_mean = numberOfValidDuration == 0 ? 0 : round(duration_sum / numberOfValidDuration);
                // 时间记录
                duration_sum = 0;
                // 有效时间数据个数
                numberOfValidDuration = 0;
            }

            // 保存这个点运行需要的时间
            sg805RobotPtr->trajectory[index].duration = duration_mean;
            duration_total += duration_mean;

            std::cout << "第 " << index << "个点"
                      << " duration " << duration_mean / 200 << "us, "
                      << (double)duration_mean / 200000000 << "s" << endl;
            /*******************************************************
             *        将路径点赋值 角度获取 单点执行时间获取结束          *
             *******************************************************/

            /*******************************************************
             *   当所有点执行需要的时间都获取之后，开始获取速度            *
             *******************************************************/

            // 第一个点,没有运行速度, 在前面 if (index == 0) 中已经设定完毕
            if (index != 0)
            {

                for (int i = 0; i < 6; i++)
                {
                    // 只有速度不为0，且发生了位置偏移，才进行脉冲周期计算
                    if (goal->trajectory.points[index].velocities[i] != 0 && delta_postions[i] != 0)
                    {
                        // 周期 = 运行时间/脉冲差
                        sg805RobotPtr->trajectory[index].period[i] = abs(duration_mean / delta_postions[i]);

                        subVelocityModule(index, i);
                    }
                    // 如果此关节在此点运行过程中 没有任何位置移动，则在此周期内，不允许此关节发生移动
                    else if (delta_postions[i] == 0)
                    {
                        sg805RobotPtr->trajectory[index].period[i] = duration_mean;
                        subVelocityModule(index, i);
                    }
                }
            }
        }

    }

    // 调用smallrobotRobot中的sendTrajectory进行发送数据的操作
    
    // sg805RobotPtr->printTrajectory();// 打印轨迹信息
    sg805RobotPtr->sendTrajectory();

    gettimeofday(&tStart, 0);
    // 等待状态变化 无需特别高的实时性
    usleep(500000); // 至少等待0.5s
    while (!sg805RobotPtr->isArrived())
    {
        usleep(10000); // 等待0.1s
    }

    gettimeofday(&tEnd, 0);
    // 动作完成，反馈结果，设置完成状态
    ros_result.error_code = ros_result.SUCCESSFUL;
    as->setSucceeded(ros_result);

    duration_total_actual = ((tEnd.tv_sec - tStart.tv_sec) * 1000000 + tEnd.tv_usec - tStart.tv_usec);

    std::cout << "路径执行完成" << std::endl;
    std::cout << "预计使用" << (double)duration_total / 200000000 << "s, 实际使用"
              << (double)duration_total_actual / 1000000 << "s" << std::endl;

              
    sg805RobotPtr->getPose();
    usleep(100000); // 0.1s间隔
    // 写入到控制器中
    sg805RobotPtr->location_setting();

    usleep(100000); // 0.1s间隔
}

// 向ros系统中更新关节状态
void SG805RobotRos::jointStateUpdate()
{
    while (ros::ok())
    {
        for (int i = 0; i < 6; i++)
        {
            // if (i == 5)
            // {   //2023-6-3
            //     sg805RobotPtr->location.position[5] -= sg805RobotPtr->location.position[4] / 50; 
            // }

            // // joint_msg.position[i] = (sg805RobotPtr->location.position[i] - sg805RobotPtr->zeroPlu[i]) * PI / sg805RobotPtr->plu2angel[i];
            // joint_msg.position[i] = (sg805RobotPtr->location.position[i] - sg805RobotPtr->zeroPlu[i]) * PI / sg805RobotPtr->plu2angel[i];

            if (i != 5)
            {
                joint_msg.position[i] = (sg805RobotPtr->location.position[i] - sg805RobotPtr->zeroPlu[i]) * PI / sg805RobotPtr->plu2angel[i];
            }
            else if( i == 5)
            {
                //2023-6-3
                joint_msg.position[5] = (sg805RobotPtr->location.position[5] - sg805RobotPtr->zeroPlu[5] - sg805RobotPtr->location.position[4] / 50 ) 
                    * PI / sg805RobotPtr->plu2angel[5];
            }




            /**********************************
             * 2023-05-28 注释此部分
            // 角度控制在-PI到+PI之间
            // while (joint_msg.position[i] >= PI)
            // {
            //     joint_msg.position[i] = joint_msg.position[i] - 2 * PI;
            // }

            // while (joint_msg.position[i] <= -PI)
            // {
            //     joint_msg.position[i] = joint_msg.position[i] + 2 * PI;
            // }
             *
             **********************************/


            

            ros_feedback.actual.positions[i] = joint_msg.position[i];
        }
        joint_msg.header.stamp = ros::Time::now();
        joint_pub.publish(joint_msg);

        //cout << "joint_state  " << joint_msg.header.stamp << endl;

        if (sg805RobotPtr->location.state == RUNING)
        {
            ros_feedback.header.stamp = ros::Time::now();
            as->publishFeedback(ros_feedback);
        }

        usleep(100000);
    }
}

// 重排序，urdf设计的顺序比较好，可以不用这个
void SG805RobotRos::reorder(trajectory_msgs::JointTrajectory trajectory)
{
}

// 辅助运算模块
void SG805RobotRos::subVelocityModule(int pointIndex, int jointIndex)
{
    if (sg805RobotPtr->trajectory[pointIndex].period[jointIndex] <= 1200000)
    {
        sg805RobotPtr->trajectory[pointIndex].restPeriod[jointIndex] = 99;

        sg805RobotPtr->trajectory[pointIndex].numberOfFullPeriod[jointIndex] =
            sg805RobotPtr->trajectory[pointIndex].period[jointIndex] / 200 - 1;

        sg805RobotPtr->trajectory[pointIndex].numberOfPeriod[jointIndex] = duration_mean / 200 -
                                                                           delta_postions[jointIndex] * sg805RobotPtr->trajectory[pointIndex].numberOfFullPeriod[jointIndex];

        std::cout << "小数算法" << std::endl;
    }
    else if (sg805RobotPtr->trajectory[pointIndex].period[jointIndex] > 1200000 && sg805RobotPtr->trajectory[pointIndex].period[jointIndex] <= 3600000000)
    {
        sg805RobotPtr->trajectory[pointIndex].restPeriod[jointIndex] =
            (int)__builtin_sqrt(sg805RobotPtr->trajectory[pointIndex].period[jointIndex] / 2);

        sg805RobotPtr->trajectory[pointIndex].numberOfFullPeriod[jointIndex] =
            sg805RobotPtr->trajectory[pointIndex].restPeriod[jointIndex];

        sg805RobotPtr->trajectory[pointIndex].numberOfPeriod[jointIndex] = 0;

        std::cout << "大数算法" << std::endl;
    }
    else
    {
        // 无法作出更慢的动作了
        sg805RobotPtr->trajectory[pointIndex].restPeriod[jointIndex] = 60000;
        sg805RobotPtr->trajectory[pointIndex].numberOfFullPeriod[jointIndex] = 60000;
        sg805RobotPtr->trajectory[pointIndex].numberOfPeriod[jointIndex] = 0;
    }
}

// 发送串口
void SG805RobotRos::usart_send()
{
    // 根据编码器工厂进行通信设计，如果编码器是被动方式，数据放入到缓存中
    char s[18] = "Hello world ros!\n";
    sg805RobotPtr->usart_tx_len = 17;
    memcpy(sg805RobotPtr->usartTXBuffer, s, sg805RobotPtr->usart_tx_len);

    // 调用发送
    sg805RobotPtr->usart_send();

    sleep(1);
}

// 关闭串口
void SG805RobotRos::usart_close()
{
    // 关闭串口
    sg805RobotPtr->usart_stop();

    sleep(1);

    // 开启Location上传
    sg805RobotPtr->upload_start();
}


bool SG805RobotRos::getDistanceCB(sg805_driver::getDistance::Request &req, 
                    sg805_driver::getDistance::Response &res)
{
    switch (req.category)
    {
    case 0:
        // 获取距离
        res.data = sg805RobotPtr->getDistanceSensorData(req.id);
        break;
    
    default:
        break;
    }    
    return true;
}



// 测试
void SG805RobotRos::test()
{
}